#ifndef SIMPLEPID_H  // 防止重复包含
#define SIMPLEPID_H

#include <algorithm>  // for std::clamp
#include <cmath>      // for std::fabs
using namespace std;
// 自定义 PID 控制器类（简化版，仅含 P+D）
class SimplePID {
public:
    SimplePID(double kp, double kd) : kp_(kp), kd_(kd), prev_error_(0.0) {}
    
    double compute(double error, double dt) {
        double derivative = (error - prev_error_) / dt;
        prev_error_ = error;
        return kp_ * error + kd_ * derivative;
    }

private:
    double kp_;   // 比例增益
    double kd_;   // 微分增益
    double prev_error_;  // 上一帧误差
};

#endif // FORMATION_CTRL_NODE_H